#define CHECK_NUMBER

using System;

namespace FixedPointMath
{
    public struct Quaternion
    {
        public Number x;
        public Number y;
        public Number z;
        public Number w;

        private static readonly Quaternion identityQuaternion = new Quaternion(Number.Zero, Number.Zero, Number.Zero, Number.N1);
        public static Quaternion identity
        {
            get
            {
                return identityQuaternion;
            }
        }

        public Vector3 eulerAngles
        {
            get
            {
                Matrix4x4 m = QuaternionToMatrix(this);
                return MatrixToEuler(m) * Number.R180 / Number.PI;
            }
            set
            {
                this = Euler(value);
            }
        }

        public Number this[int index]
        {
            get
            {
                switch (index)
                {
                    case 0: return x;
                    case 1: return y;
                    case 2: return z;
                    case 3: return w;
                    default:
                        throw new IndexOutOfRangeException("Invalid NumberQuaternion get index!");
                }
            }
            set
            {
                switch (index)
                {
                    case 0: x = value; break;
                    case 1: y = value; break;
                    case 2: z = value; break;
                    case 3: w = value; break;
                    default:
                        throw new IndexOutOfRangeException("Invalid NumberQuaternion set index!");
                }
            }
        }

        public Quaternion(Number x, Number y, Number z, Number w)
        {
            this.x = x;
            this.y = y;
            this.z = z;
            this.w = w;
        }

        /// <summary>
        /// Is the dot product of two quaternions within tolerance for them to be considered equal?
        /// </summary>
        /// <param name="dot"></param>
        /// <returns></returns>
        private static bool IsEqualUsingDot(Number dot)
        {
            return dot > Number.N1 - Number.OneOf1000000;
        }

        #region static method
        /// <summary>
        /// 点乘
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static Number Dot(Quaternion a, Quaternion b)
        {
            return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
        }

        /// <summary>
        /// 夹角
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static Number Angle(Quaternion a, Quaternion b)
        {
            Number dot = Dot(a, b);
            dot = dot.Abs();
            return MathFP.AcosDegree(MathFP.Min(dot, Number.N1)) * Number.R2;
        }

        /// <summary>
        /// 轴向旋转
        /// </summary>
        /// <param name="degree"></param>
        /// <param name="axis"></param>
        /// <returns></returns>
        public static Quaternion AngleAxis(Number degree, Vector3 axis)
        {
            axis = axis.normalized;

            var halfDegree = degree / Number.R2;
            Number sin = MathFP.SinDegree(halfDegree);
            return new Quaternion(sin * axis.x, sin * axis.y, sin * axis.z, MathFP.CosDegree(halfDegree));
        }

        /// <summary>
        /// 欧拉角转四元数
        /// </summary>
        /// <param name="euler"></param>
        /// <returns></returns>
        public static Quaternion Euler(Vector3 euler)
        {
            return Euler(euler.x, euler.y, euler.z);
        }
        /// <summary>
        /// 欧拉角转四元数
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="z"></param>
        /// <returns></returns>
        public static Quaternion Euler(Number x, Number y, Number z)
        {
            Number cX = MathFP.CosDegree(x / Number.R2);
            Number sX = MathFP.SinDegree(x / Number.R2);

            Number cY = MathFP.CosDegree(y / Number.R2);
            Number sY = MathFP.SinDegree(y / Number.R2);

            Number cZ = MathFP.CosDegree(z / Number.R2);
            Number sZ = MathFP.SinDegree(z / Number.R2);

            Quaternion qX = new Quaternion(sX, Number.Zero, Number.Zero, cX);
            Quaternion qY = new Quaternion(Number.Zero, sY, Number.Zero, cY);
            Quaternion qZ = new Quaternion(Number.Zero, Number.Zero, sZ, cZ);

            Quaternion q = (qY * qX) * qZ;

            return q;
        }

        /// <summary>
        /// 向目标角度旋转
        /// </summary>
        /// <param name="from"></param>
        /// <param name="to"></param>
        /// <param name="maxDegreesDelta"></param>
        /// <returns></returns>
        public static Quaternion RotateTowards(Quaternion from, Quaternion to, Number maxDegreesDelta)
        {
            Number n = Quaternion.Angle(from, to);
            Quaternion result = new Quaternion();
            if (n == Number.Zero)
            {
                result = to;
            }
            else
            {
                Number t = MathFP.Min(Number.N1, maxDegreesDelta / n);
                result = Quaternion.SlerpUnclamped(from, to, t);
            }
            return result;
        }

        /// <summary>
        /// 四元数的逆
        /// </summary>
        /// <param name="rotation"></param>
        /// <returns></returns>
        public static Quaternion Inverse(Quaternion rotation)
        {
            return new Quaternion(-rotation.x, -rotation.y, -rotation.z, rotation.w);
        }

        /// <summary>
        /// 线性插值
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="t"></param>
        /// <returns></returns>
        public static Quaternion Lerp(Quaternion a, Quaternion b, Number t)
        {
            if (t > Number.N1)
            {
                t = Number.N1;
            }
            if (t < Number.Zero)
            {
                t = Number.Zero;
            }
            return LerpUnclamped(a, b, t);
        }
        /// <summary>
        /// 线性插值(无限制)
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="t"></param>
        /// <returns></returns>
        public static Quaternion LerpUnclamped(Quaternion a, Quaternion b, Number t)
        {
            Quaternion tmpQuat = new Quaternion();
            if (Dot(a, b) < Number.Zero)
            {
                tmpQuat.Set(a.x + t * (-b.x - a.x),
                            a.y + t * (-b.y - a.y),
                            a.z + t * (-b.z - a.z),
                            a.w + t * (-b.w - a.w));
            }
            else
            {
                tmpQuat.Set(a.x + t * (b.x - a.x),
                            a.y + t * (b.y - a.y),
                            a.z + t * (b.z - a.z),
                            a.w + t * (b.w - a.w));
            }
            Number nor = MathFP.Sqrt(Dot(tmpQuat, tmpQuat));
            return new Quaternion(tmpQuat.x / nor, tmpQuat.y / nor, tmpQuat.z / nor, tmpQuat.w / nor);
        }

        /// <summary>
        /// 球形插值
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="t"></param>
        /// <returns></returns>
        public static Quaternion Slerp(Quaternion a, Quaternion b, Number t)
        {
            if (t > Number.N1)
            {
                t = Number.N1;
            }
            if (t < Number.Zero)
            {
                t = Number.Zero;
            }
            return SlerpUnclamped(a, b, t);
        }
        /// <summary>
        /// 球形插值(无限制)
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <param name="t"></param>
        /// <returns></returns>
        public static Quaternion SlerpUnclamped(Quaternion q1, Quaternion q2, Number t)
        {
            Number dot = Dot(q1, q2);

            Quaternion tmpQuat = new Quaternion();
            if (dot < Number.Zero)
            {
                dot = -dot;
                tmpQuat.Set(-q2.x, -q2.y, -q2.z, -q2.w);
            }
            else
            {
                tmpQuat = q2;
            }

            if (dot < Number.N1)
            {
                Number radin = MathFP.AcosRadin(dot);
                Number sinadiv, sinat, sinaomt;
                sinadiv = Number.N1 / MathFP.SinRadin(radin);
                sinat = MathFP.SinRadin(radin * t);
                sinaomt = MathFP.SinRadin(radin * (Number.N1 - t));
                tmpQuat.Set((q1.x * sinaomt + tmpQuat.x * sinat) * sinadiv,
                         (q1.y * sinaomt + tmpQuat.y * sinat) * sinadiv,
                         (q1.z * sinaomt + tmpQuat.z * sinat) * sinadiv,
                         (q1.w * sinaomt + tmpQuat.w * sinat) * sinadiv);
                return tmpQuat;
            }
            else
            {
                return Lerp(q1, tmpQuat, t);
            }
        }

        /// <summary>
        /// 朝向旋转
        /// </summary>
        /// <param name="forward"></param>
        /// <returns></returns>
        public static Quaternion LookRotation(Vector3 forward)
        {
            Vector3 up = Vector3.Up;
            return LookRotation(forward, up);
        }
        /// <summary>
        /// 朝向旋转
        /// </summary>
        /// <param name="forward"></param>
        /// <param name="upwards"></param>
        /// <returns></returns>
        public static Quaternion LookRotation(Vector3 forward, Vector3 upwards)
        {
            Matrix4x4 m = LookRotationToMatrix(forward, upwards);
            return MatrixToQuaternion(m);
        }
        #endregion

        #region instance method
        /// <summary>
        /// 设置四元数
        /// </summary>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="z"></param>
        /// <param name="w"></param>
        public void Set(Number _x, Number _y, Number _z, Number _w)
        {
            x = _x;
            y = _y;
            z = _z;
            w = _w;
        }

        /// <summary>
        /// 设置朝向旋转
        /// </summary>
        /// <param name="view"></param>
        public void SetLookRotation(Vector3 view)
        {
            this = LookRotation(view);
        }
        /// <summary>
        /// 设置朝向旋转
        /// </summary>
        /// <param name="view"></param>
        /// <param name="up"></param>
        public void SetLookRotation(Vector3 view, Vector3 up)
        {
            this = LookRotation(view, up);
        }

        /// <summary>
        /// 转换为角轴
        /// </summary>
        /// <param name="degree"></param>
        /// <param name="axis"></param>
        public void ToAngleAxis(out Number degree, out Vector3 axis)
        {
            degree = Number.R2 * MathFP.AcosDegree(w);
            if (degree == Number.Zero)
            {
                axis = Vector3.Right;
                return;
            }

            Number div = Number.N1 / MathFP.Sqrt(Number.N1 - w * w);
            axis = new Vector3();
            axis.Set(x * div, y * div, z * div);
        }
        #endregion

        #region operator
        /// <summary>
        /// 
        /// </summary>
        /// <param name="lhs"></param>
        /// <param name="rhs"></param>
        /// <returns></returns>
        public static Quaternion operator *(Quaternion lhs, Quaternion rhs)
        {
            return new Quaternion(
                lhs.w * rhs.x + lhs.x * rhs.w + lhs.y * rhs.z - lhs.z * rhs.y,
                lhs.w * rhs.y + lhs.y * rhs.w + lhs.z * rhs.x - lhs.x * rhs.z,
                lhs.w * rhs.z + lhs.z * rhs.w + lhs.x * rhs.y - lhs.y * rhs.x,
                lhs.w * rhs.w - lhs.x * rhs.x - lhs.y * rhs.y - lhs.z * rhs.z);
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="rotation"></param>
        /// <param name="vector"></param>
        /// <returns></returns>
        public static Vector3 operator *(Quaternion rotation, Vector3 vector)
        {
            Number x = rotation.x * Number.R2;
            Number y = rotation.y * Number.R2;
            Number z = rotation.z * Number.R2;
            Number xx = rotation.x * x;
            Number yy = rotation.y * y;
            Number zz = rotation.z * z;
            Number xy = rotation.x * y;
            Number xz = rotation.x * z;
            Number yz = rotation.y * z;
            Number wx = rotation.w * x;
            Number wy = rotation.w * y;
            Number wz = rotation.w * z;

            Vector3 res;
            res.x = (Number.N1 - (yy + zz)) * vector.x + (xy - wz) * vector.y + (xz + wy) * vector.z;
            res.y = (xy + wz) * vector.x + (Number.N1 - (xx + zz)) * vector.y + (yz - wx) * vector.z;
            res.z = (xz - wy) * vector.x + (yz + wx) * vector.y + (Number.N1 - (xx + yy)) * vector.z;
            return res;
        }

        /// <summary>
        ///
        /// </summary>
        /// <param name="lhs"></param>
        /// <param name="rhs"></param>
        /// <returns></returns>
        public static bool operator ==(Quaternion lhs, Quaternion rhs)
        {
            return IsEqualUsingDot(Dot(lhs, rhs));
        }

        public static bool operator !=(Quaternion lhs, Quaternion rhs)
        {
            return !IsEqualUsingDot(Dot(lhs, rhs));
        }

        public override int GetHashCode()
        {
            return x.GetHashCode() ^ y.GetHashCode() << 2 ^ z.GetHashCode() >> 2 ^ w.GetHashCode() >> 1;
        }
        public override bool Equals(object other)
        {
            if (other is Quaternion)
            {
                var oth = (Quaternion)other;
                return this == oth;
            }
            return false;
        }
        #endregion

        #region private functions
        private Vector3 MatrixToEuler(Matrix4x4 m)
        {
            Vector3 v = new Vector3();
            if (m[1, 2] < Number.N1)
            {
                if (m[1, 2] > Number.NN1)
                {
                    v.x = MathFP.AsinRadin(-m[1, 2]);
                    v.y = MathFP.Atan2Radin(m[0, 2], m[2, 2]);
                    v.z = MathFP.Atan2Radin(m[1, 0], m[1, 1]);
                }
                else
                {
                    v.x = Number.HalfPI;
                    v.y = MathFP.Atan2Radin(m[0, 1], m[0, 0]);
                    v.z = Number.Zero;
                }
            }
            else
            {
                v.x = -Number.HalfPI;
                v.y = MathFP.Atan2Radin(-m[0, 1], m[0, 0]);
                v.z = Number.Zero;
            }

            for (int i = 0; i < 3; i++)
            {
                if (v[i] < Number.Zero)
                {
                    v[i] += Number.DoublePI;
                }
                else if (v[i] > Number.DoublePI)
                {
                    v[i] -= Number.DoublePI;
                }
            }

            return v;
        }
        public static Matrix4x4 QuaternionToMatrix(Quaternion quat)
        {
            Matrix4x4 m = new Matrix4x4();

            Number x = quat.x * Number.R2;
            Number y = quat.y * Number.R2;
            Number z = quat.z * Number.R2;
            Number xx = quat.x * x;
            Number yy = quat.y * y;
            Number zz = quat.z * z;
            Number xy = quat.x * y;
            Number xz = quat.x * z;
            Number yz = quat.y * z;
            Number wx = quat.w * x;
            Number wy = quat.w * y;
            Number wz = quat.w * z;

            m[0] = Number.N1 - (yy + zz);
            m[1] = xy + wz;
            m[2] = xz - wy;
            m[3] = Number.Zero;

            m[4] = xy - wz;
            m[5] = Number.N1 - (xx + zz);
            m[6] = yz + wx;
            m[7] = Number.Zero;

            m[8] = xz + wy;
            m[9] = yz - wx;
            m[10] = Number.N1 - (xx + yy);
            m[11] = Number.Zero;

            m[12] = Number.Zero;
            m[13] = Number.Zero;
            m[14] = Number.Zero;
            m[15] = Number.N1;

            return m;
        }
        private static Quaternion MatrixToQuaternion(Matrix4x4 m)
        {
            Quaternion quat = new Quaternion();

            Number fTrace = m[0, 0] + m[1, 1] + m[2, 2];
            Number root;

            if (fTrace > Number.Zero)
            {
                root = MathFP.Sqrt(fTrace + Number.N1);
                quat.w = root / Number.R2;
                root = Number.OneOf2 / root;
                quat.x = (m[2, 1] - m[1, 2]) * root;
                quat.y = (m[0, 2] - m[2, 0]) * root;
                quat.z = (m[1, 0] - m[0, 1]) * root;
            }
            else
            {
                int[] s_iNext = new int[] { 1, 2, 0 };
                int i = 0;
                if (m[1, 1] > m[0, 0])
                {
                    i = 1;
                }
                if (m[2, 2] > m[i, i])
                {
                    i = 2;
                }
                int j = s_iNext[i];
                int k = s_iNext[j];

                root = MathFP.Sqrt(m[i, i] - m[j, j] - m[k, k] + 1);
                if (root < Number.Zero)
                {
                    throw new IndexOutOfRangeException("MatrixToQuaternion error!");
                }
                quat[i] = root / Number.R2;
                root = Number.OneOf2 / root;
                quat.w = (m[k, j] - m[j, k]) * root;
                quat[j] = (m[j, i] + m[i, j]) * root;
                quat[k] = (m[k, i] + m[i, k]) * root;
            }
            Number nor = MathFP.Sqrt(Dot(quat, quat));
            quat = new Quaternion(quat.x / nor, quat.y / nor, quat.z / nor, quat.w / nor);

            return quat;
        }
        private static Matrix4x4 LookRotationToMatrix(Vector3 viewVec, Vector3 upVec)
        {
            Vector3 z = viewVec;
            Matrix4x4 m = new Matrix4x4();

            Number mag = z.magnitude;
            if (mag < Number.Zero)
            {
                m = Matrix4x4.identity;
            }
            z /= mag;

            Vector3 x = MathFP.Cross(upVec, z);
            mag = x.magnitude;
            if (mag < Number.Zero)
            {
                m = Matrix4x4.identity;
            }
            x /= mag;

            Vector3 y = MathFP.Cross(z, x);

            m[0, 0] = x.x; m[0, 1] = y.x; m[0, 2] = z.x;
            m[1, 0] = x.y; m[1, 1] = y.y; m[1, 2] = z.y;
            m[2, 0] = x.z; m[2, 1] = y.z; m[2, 2] = z.z;

            return m;
        }
        #endregion
    }
}
